#include "Stopper.h"
#include "geometry_msgs/Twist.h"
Stopper::Stopper()
{
    keepMoving=1;
    commandPub = node.advertise<geometry_msgs::Twist>("/cmd_vel",10);
    laserSub=node.subscribe("/scan",1,&Stopper::scanCallback,this);
}

void Stopper::moveForward()
{
    geometry_msgs::Twist msg;
    msg.linear.x=FORWARD_SPEED *keepMoving;
    ROS_INFO("Speed:%lf",msg.linear.x);
    commandPub.publish(msg);
}

void Stopper::moveForward_2()
{
    geometry_msgs::Twist msg3;
    msg3.linear.x=FORWARD_SPEED_2*keepMoving;
    commandPub.publish(msg3);
}

void Stopper::turn()
{
    geometry_msgs::Twist msg2;
    msg2.angular.z=TURN_SPEED * keepMoving;
    ROS_INFO("Speed:%lf",msg2.angular.z);
    commandPub.publish(msg2);
}

void Stopper::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    bool isObstacleInFront = false;
    int minIndex = 360 + floor((MIN_SCAN_ANGLE - scan->angle_min) / scan->angle_increment);
    int maxIndex = floor((MAX_SCAN_ANGLE - scan->angle_min) / scan->angle_increment);
    std::vector<std::pair<float, float>> scaled_ranges;
    for (int i = minIndex; i < 360; i++)
    scaled_ranges.push_back(std::make_pair(i, scan->ranges[i]));
    for (int i = 0; i <= maxIndex; i++)
    scaled_ranges.push_back(std::make_pair(i, scan->ranges[i]));
    for(auto value : scaled_ranges)
    {
        if (value.second < MIN_DIST_FROM_OBSTACLE)
        {
            keepMoving = 2;
            ROS_INFO("I'll turn an angle");
        }
    }
}

void Stopper::startMoving()
{
    ros::Rate rate(10);
    ROS_INFO("Start moving!");
    while (ros::ok())
    {
        ros::spinOnce();
        moveForward();
        if(keepMoving==2)
        {
            turn();
            rate.sleep();
            moveForward_2();
            keepMoving-=1;
        }
        rate.sleep();
    }
}